#include "driver/twai.h"
#include "Motor.h"
#include "string.h"
typedef union EncoderData
{
    uint8_t data[8];
    struct _encoderData
    {
        float pos;
        float speed;
    } encoderData;
    
    /* data */
}EncoderData;

uint8_t CanInit(void){
    twai_general_config_t g_config = {
		.mode = TWAI_MODE_NORMAL , //TWAI_MODE_NORMAL / TWAI_MODE_NO_ACK / TWAI_MODE_LISTEN_ONLY
		.tx_io = TX_GPIO_PIN, //IO号
		.rx_io = RX_GPIO_PIN, //IO号
        .clkout_io = TWAI_IO_UNUSED, //io号，不用为-1
        .bus_off_io = TWAI_IO_UNUSED,//io号，不用为-1
        .tx_queue_len = 5, //发送队列长度，0-禁用发送队列
        .rx_queue_len = 5,//接收队列长度
        .alerts_enabled = TWAI_ALERT_NONE,  //警告标志 TWAI_ALERT_ALL 可开启所有警告
        .clkout_divider = 0,//1 to 14 , 0-不用
        .intr_flags = ESP_INTR_FLAG_LEVEL1
        };//中断优先级
        
//CAN接口时序配置官方提供了1K to 1Mbps的常用配置
twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS(); //TWAI_TIMING_CONFIG_500KBITS()

//过滤器配置
twai_filter_config_t f_config = {
		.acceptance_code = 0, //验证代码
		.acceptance_mask = 0xFFFFFFFF, //验证掩码 0xFFFFFFFF表示全部接收
		.single_filter = true//true：单过滤器模式 false：双过滤器模式
        };
    if(twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK)
    {
        return 0;
    }
    if(twai_start() != ESP_OK)
    {
        return 0;
    }
    return 1;
   

}
void CanRecvMessage(void){
    twai_status_info_t _statusInfo;
    twai_get_status_info(&_statusInfo); //获取CAN状态信息
    twai_message_t rxMsg; //接收消息结构体
    
    while(_statusInfo.msgs_to_rx > 0){ //接收队列中有消息时，读取消息
        if (twai_receive(&rxMsg, 0) == ESP_OK) { //读取消息，超时时间1000ms
            uint8_t NodeID = rxMsg.identifier >> 5;
            uint8_t CmdID = rxMsg.identifier & 0x1F;
            //printf("%d\n",CmdID); //打印命令ID
            if(NodeID == 0x00){
                switch (CmdID)
                {
                    case 0x09: //编码器位置
                        EncoderData _encoderData;
                        memcpy(&_encoderData.data,&rxMsg.data,8);
                        motor1.Position = _encoderData.encoderData.pos;
                        motor1.Speed = _encoderData.encoderData.speed;
                        break;
                    
                    default:
                        break;
                }
            }
        }
        twai_get_status_info(&_statusInfo);
    }
}
void CanSendMessageWithRemoteFrame(uint32_t id, uint8_t *data, uint8_t len){
    twai_message_t Message2Send;
    Message2Send.extd = 0;
    Message2Send.self = 0;
    Message2Send.rtr  = 1;
    Message2Send.ss   = 0;
    Message2Send.dlc_non_comp = 0;
    
    Message2Send.identifier = id; //标识符
    Message2Send.data_length_code = len; //数据长度8字节
    memcpy((void*)Message2Send.data, (void*)data, len); //数据内容 
    esp_err_t ret = twai_transmit(&Message2Send, 0);
    switch (ret)
    {
        // case ESP_OK:
        //    printf("Message sent\r\n");
        //    break;
        case ESP_ERR_INVALID_ARG:
            printf("Invalid argument\r\n");
            break;
        case ESP_ERR_INVALID_STATE:
            printf("Invalid state\r\n");
            break;
        case ESP_ERR_TIMEOUT:
            printf("Timeout\r\n");
            break;

        default:
            break;
    }
    
}
void CanSendMessage(uint8_t node_id,uint8_t cmd, uint8_t *data, uint8_t len){
    twai_message_t Message2Send;
    Message2Send.extd = 0;
    Message2Send.self = 0;
    Message2Send.rtr  = 0;
    Message2Send.ss   = 0;
    Message2Send.dlc_non_comp = 0;
    
    Message2Send.identifier = (uint32_t)(node_id<<5|cmd); //标识符
    Message2Send.data_length_code = len; //数据长度8字节
    memcpy((void*)Message2Send.data, (void*)data, len); //数据内容 
    esp_err_t ret = twai_transmit(&Message2Send, 0);
}